classdef nlSLIP
    %UNTITLED2 Summary of this class goes here
    %   Detailed explanation goes here
    
    properties
       params; 
       intprops;
       state;
       data;
    end
    
    methods
        function obj=nlSLIP()
            obj.params.krot=1800; %Nm/rad
            obj.params.s=0.5;   %m
            obj.params.m=58;    %kg
            obj.params.k=22393; %N/m
            obj.params.l0=0.85;
            obj.params.legs=1;
            obj.params.d=0.0;
            obj.params.g=9.81;
            obj.intprops.abstol=1e-12;
            obj.intprops.reltol=1e-12;
            obj.intprops.ie=[];
            obj.intprops.nl=1;
            obj.intprops.apex=1;
            obj.intprops.ltp=-1;            %ydot -1: do not trigger, 1 or 0: trigger on
            obj.intprops.vlo=1;               %xFP-x 1: do not trigger, -1 or 0: trigger on
            obj.state.t=[];
            obj.state.x=[];
            obj.state.xe=[0 0 0.9 0];
            obj.state.IC=obj.state.xe;
            obj.state.te=0;
            obj.state.FP=[0 0 NaN];
            obj.params.aTD=pi/2;
        end
    
        %% Integrator
        function obj = SLIPintegrate(obj)
            [t, x, obj.state.te, obj.state.xe, obj.intprops.ie] = ode45(obj.intprops.state_fun,...
                obj.state.te:0.001:obj.state.te+3, obj.state.xe, obj.intprops.options);
            obj.state.t = [obj.state.t;t];
            obj.state.x = [obj.state.x;x];
        end
        
        %% EOM for motion phases
        
        %EOM flight phase
        function xdot = flight(obj,~,x)
            xdot = [x(2) 0 x(4) -obj.params.g]';
        end
        
        function xdot = single_support(obj,~,x)
            L = sqrt( (x(1)-obj.state.FP(end,1))^2 + (x(3)-obj.state.FP(end,2))^2 );
            F = obj.params.legs*obj.params.krot/obj.params.s*(acos(L/2/obj.params.s)-acos(obj.params.l0/2/obj.params.s))/sqrt(1-(L/2/obj.params.s)^2);
            if obj.intprops.nl
                %% nonlinear
                xdot = [x(2); ...
                    F/obj.params.m*(x(1)-obj.state.FP(end,1))/L; ...   
                    x(4); ...
                    F/obj.params.m*(x(3)-obj.state.FP(end,2))/L-obj.params.g];
            else 
                %%linear hopping
                if abs(x(1))==0
                    xdot = [0.0; 0.0; x(4); obj.params.k/obj.params.m*(obj.params.l0-x(3))-obj.params.g];
                else
                %% linear general
                    xdot = [x(2); obj.params.k/obj.params.m*(obj.params.l0-L)*(x(1)-obj.state.FP(end,1))/L; x(4); obj.params.k/obj.params.m*(obj.params.l0-L)*(x(3)-obj.state.FP(end,2))/L-obj.params.g];
                    disp('|vx|>0')
                end
            end
            %plot(x(1),x(3),'*')
        end
        
        function xdot = double_support(obj,~,x)
            L = sqrt(sum((ones(2,1)*x([1 3])'-obj.state.FP(end-1:end,1:2)).^2,2));
            F = obj.params.legs*obj.params.krot/obj.params.s*(acos(L/2/obj.params.s)-acos(obj.params.l0/2/obj.params.s))./sqrt(1-(L/2/obj.params.s).^2);
            F_lin = obj.params.k*(obj.params.l0-L);
            if obj.intprops.nl
                %% nonlinear
                xdot = [x(2); ...
                    sum(F/obj.params.m.*(x(1)-obj.state.FP(end-1:end,1))./L);
                    x(4);
                    sum(F/obj.params.m.*(x(3)-obj.state.FP(end-1:end,2))./L)-obj.params.g];
            else  %% linear
                xdot = [x(2); ...
                    sum(F_lin/obj.params.m.*(x(1)-obj.state.FP(end-1:end,1))./L);
                    x(4);
                    sum(F_lin/obj.params.m.*(x(3)-obj.state.FP(end-1:end,2))./L)-obj.params.g];
            end
        end
        
       
        
        %% Hopping spring mass model
       
        %event handler flight phase hopping
        function [value, isterminal, direction] = hop_flight_events(obj, ~, x)
            value = obj.params.l0-x(3);     % detect TD height
            isterminal = 1;          % stop the integration
            direction = 1;          % negative direction
        end
        
        %event handler stance phase hopping
        function [value, isterminal, direction] = hop_stance_events(obj, ~, x)
            value = obj.params.l0-x(3);     % detect TO height
            isterminal = 1;          % stop the integration
            direction = -1;           % positive direction
        end
        
        %% walking spring mass model
        function [value, isterminal, direction] = walk_single_events(obj, ~, x)
            value = [x(3)-obj.params.l0*sin(obj.params.aTD); ...
                obj.state.FP(end,1)-x(1); ...   %VLO
                x(3); ...
                x(2)-1e-3; ...
                x(3)-obj.params.l0
                x(4)-1e-6];
            isterminal = ones(size(value));
            direction = [-1 obj.intprops.vlo 0 0 1 -1]';  
          end
        
        %event handler double support phase
        function [value, isterminal, direction] = walk_double_events(obj, ~, x)
           L = sqrt(sum((ones(2,1)*x([1 3])'-obj.state.FP(end-1:end,1:2)).^2,2));
           value = [obj.params.l0 - L(1); ...   %TO
               x(4); ...                        %LTP
               x(3); ...                        %falling
               x(2)-1e-3; ...                   %horizontal velocity
               obj.params.l0 - L(2)];           %leading leg TO
           isterminal = ones(size(value));
           direction = [-1 obj.intprops.ltp 0 0 -1]';
        end
        
        %% call motion phases
        % call flight phase integration
        function obj = int_hop_flight(obj)
            obj.intprops.options = odeset('RelTol', obj.intprops.reltol, 'AbsTol', obj.intprops.abstol, 'Events', @obj.hop_flight_events);
            obj.intprops.state_fun = @obj.flight;
            obj = SLIPintegrate(obj);
            obj.state.FP=[obj.state.FP;obj.state.xe(1)+obj.params.l0*cos(obj.params.aTD) obj.state.xe(3)-obj.params.l0*sin(obj.params.aTD) obj.state.te];
        end
        
        %call stance phase integration
        function obj = int_hop_stance(obj)
            obj.intprops.options = odeset('RelTol', obj.intprops.reltol, 'AbsTol', obj.intprops.abstol, 'Events', @obj.hop_stance_events);
            obj.intprops.state_fun = @obj.single_support;
            obj = SLIPintegrate(obj);
        end
        
        function obj = int_hop_stance_compare(obj)
            obj.state.te=0;
            obj.intprops.v0=-1;
            obj.intprops.options = odeset('RelTol', obj.intprops.reltol, 'AbsTol', obj.intprops.abstol, 'Events',  @obj.hop_stance_events);
            obj.intprops.state_fun = @obj.single_support_compare;
            obj = SLIPintegrate(obj);
        end
        
        function obj = int_walk_single(obj)
            obj.intprops.options = odeset('RelTol', obj.intprops.reltol, 'AbsTol', obj.intprops.abstol, 'Events', @obj.walk_single_events);
            obj.intprops.state_fun = @obj.single_support;
            obj = SLIPintegrate(obj);
            if obj.intprops.ie==1
                obj.state.FP = [obj.state.FP;obj.state.xe(1)+obj.params.l0*cos(obj.params.aTD) obj.state.xe(3)-obj.params.l0*sin(obj.params.aTD) obj.state.te];
            end
        end
        
        function obj = int_walk_double(obj)
            obj.intprops.options = odeset('RelTol', obj.intprops.reltol, 'AbsTol', obj.intprops.abstol, 'Events', @obj.walk_double_events);
            obj.intprops.state_fun = @obj.double_support;
            obj = SLIPintegrate(obj);
        end
            
        
        function obj=data_processing(obj)
            %leg length
            phi=NaN(size(obj.state.FP,1),size(obj.state.x,1));
            x=phi;
            l=phi;
            if sum(diff(obj.state.x(:,1))>1e-15)==0     %no forward motion -> hopping
                obj.data.l=sqrt(sum((obj.state.x(:,[1 3])-ones(length(obj.state.x),1)*obj.state.FP(1,1:2)).^2,2));
                obj.data.l(obj.data.l>obj.params.l0)=obj.params.l0;
                obj.data.lphi=NaN(size(obj.data.l));
                x=obj.state.FP(1,1)-obj.state.x(:,1);
                obj.data.lphi(obj.data.l<=obj.params.l0)=acosd(x(obj.data.l<=obj.params.l0)./obj.data.l(obj.data.l<=obj.params.l0));
            else
                for idx=1:size(obj.state.FP,1)
                    l(idx,:)=sqrt(sum((obj.state.x(:,[1 3])-ones(length(obj.state.x),1)*obj.state.FP(idx,1:2)).^2,2));
                    x(idx,:)=obj.state.FP(idx,1)-obj.state.x(:,1);
                    phi(idx,l(idx,:)<=obj.params.l0)=acosd(x(idx,l(idx,:)<=obj.params.l0)./l(idx,l(idx,:)<=obj.params.l0));
                end
                l=l.*(l<=obj.params.l0)+obj.params.l0.*(l>obj.params.l0);
                obj.data.l(1,:)=min(l(1:2:end,:),[],1);
                obj.data.l(2,:)=min(l(2:2:end,:),[],1);
%                 phi1=phi(1:2:end,:);
%                 phi2=phi(2:2:end,:);
%                 obj.data.lphi=NaN(size(obj.data.l));
%                 for idx=1:length(obj.data.lphi)
%                     if sum(isfinite(phi1(:,idx)))>0
%                         try
%                             obj.data.lphi(1,idx)=phi1(isfinite(phi1(:,idx)),idx)';
%                         catch
%                             disp()
%                         end
%                     end
% %                     if sum(isfinite(phi2(idx,:)))>0
% %                         obj.data.lphi(2,idx)=phi2(isfinite(phi2(idx,:)),idx)';
% %                     end
%                 end
            end
            if obj.intprops.nl
                obj.data.F = obj.params.legs*obj.params.krot/obj.params.s.*(acos(obj.data.l/2/obj.params.s)-acos(obj.params.l0/2/obj.params.s))./sqrt(1-(obj.data.l/2/obj.params.s).^2);
                obj.data.E.spring=cumsum(obj.data.F.*gradient(obj.params.l0-obj.data.l));
            else
                obj.data.F=obj.params.k.*(obj.params.l0-obj.data.l);
                obj.data.E.spring=0.5*obj.params.k.*(obj.params.l0-obj.data.l).^2;
            end
            obj.data.E.Pot=obj.params.m*obj.params.g*obj.state.x(:,3);
            obj.data.E.Kin=1/2*obj.params.m*sum(obj.state.x(:,2).^2+obj.state.x(:,4).^2,2);
            obj.data.E.all=sum(obj.data.E.spring',2)+obj.data.E.Pot+obj.data.E.Kin;
            %obj.data.P=mean(abs(obj.data.F.*gradient(obj.state.x(:,3)))./gradient(obj.state.t));
            obj.data.tm=obj.state.t(end);
        end
        
        
    end
   
end

